import sys
import os
import time
import math
from threading import Thread
import pygame
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from ament_index_python.packages import get_package_share_directory
from inters.action import Turn, Move
from inters.srv import InfoShow, TargetUpdate, TargetRemove, TargetGet, CarInfoGet
from inters.msg import CarInfo
import getopt


def run_as_debug() -> bool:
    """
    是否传递了参数debug
    """
    debug = False
    # 获取文件名（含后缀）
    name = os.path.basename(__file__)
    try:
        """
        options, args = getopt.getopt(args, shortopts, longopts=[])

        参数args：一般是sys.argv[1:]。过滤掉sys.argv[0]，它是执行脚本的名字，不算做命令行参数。
        参数shortopts：短格式分析串。例如："hp:i:"，h后面没有冒号，表示后面不带参数；p和i后面带有冒号，表示后面带参数。
        参数longopts：长格式分析串列表。例如：["help", "ip=", "port="]，help后面没有等号，表示后面不带参数；ip和port后面带等号，表示后面带参数。

        返回值options是以元组为元素的列表，每个元组的形式为：(选项串, 附加参数)，如：('-i', '192.168.0.1')
        返回值args是个列表，其中的元素是那些不含'-'或'--'的参数。
        """
        opts, args = getopt.getopt(sys.argv[1:], "hd:", ["help", "debug="])
        # 处理 返回值options是以元组为元素的列表。
        for opt, arg in opts:
            if opt in ("-h", "--help"):
                print(f"{name} -d <True|False>")
                print(f"or: {name} --debug=<True|False>")
                sys.exit()
            elif opt in ("-d", "--debug"):
                debug = bool(arg)
        print("debug为：", debug)

        # 打印 返回值args列表，即其中的元素是那些不含'-'或'--'的参数。
        for i in range(0, len(args)):
            print("参数 %s 为：%s" % (i + 1, args[i]))
    except getopt.GetoptError:
        print(f"Error: {name} -d <True|False>")
        print(f"   or: {name} --debug=<True|False>")
    return debug


if run_as_debug():
    from car_part.map import Map
    from car_part.settings import Settings
    from car_part.model import Model
    from car_part.guid import Guid
    from car_part.target import Target
    from car_part.scaleplate import Scaleplate
else:
    from kitt.car_part.map import Map
    from kitt.car_part.settings import Settings
    from kitt.car_part.model import Model
    from kitt.car_part.guid import Guid
    from kitt.car_part.target import Target
    from kitt.car_part.scaleplate import Scaleplate


class Car:
    def __init__(self):
        self.thd_srv = Thread(target=self.start_srv)
        self.thd_srv.start()

        pygame.init()

        if run_as_debug():
            self.share_path = os.path.join(os.path.dirname(__file__), "../resource")
        else:
            self.share_path = get_package_share_directory("kitt")

        self.screen = pygame.display.set_mode((800, 600), pygame.RESIZABLE)
        pygame.display.set_caption("Car")
        self.settings = Settings("Settings")
        self.map = Map(self)
        self.model = Model(self)
        self.guid = Guid(self)
        self.target = Target(self)
        self.scaleplate = Scaleplate(self)
        self.world_angle = float(0)  # 车世界角度
        self.world_x = float(0)  # 车世界坐标x
        self.world_y = float(0)  # 车世界坐标y
        self.has_target = False

    def set_world_x(self, world_x):
        self.world_x = world_x
        self.car_srv.send_car_info(self.world_x, self.world_y, self.world_angle)

    def set_world_y(self, world_y):
        self.world_y = world_y
        self.car_srv.send_car_info(self.world_x, self.world_y, self.world_angle)

    def set_world_xy(self, world_x, world_y):
        self.world_x = world_x
        self.world_y = world_y
        self.car_srv.send_car_info(self.world_x, self.world_y, self.world_angle)
        pass

    def set_angle(self, angle):
        self.world_angle = angle
        self.guid.car_angle = self.world_angle
        self.car_srv.send_car_info(self.world_x, self.world_y, self.world_angle)

    def start_srv(self):
        rclpy.init()
        self.car_srv = CarSrv("car", self)
        rclpy.spin(self.car_srv)
        self.car_srv.destroy_node()
        rclpy.try_shutdown()

    def run_game(self):
        while True:
            self._check_events()
            self._update_screen()

    def _check_events(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                rclpy.try_shutdown()
                sys.exit()

    def _update_screen(self):
        color = self.settings.get_bg_color()
        self.screen.fill(color=color)
        self.map.update()
        self.model.update()
        if self.has_target:
            self.target.update()
        self.guid.update()
        self.scaleplate.update()
        pygame.display.flip()

    def turn(self, direction, angle):
        """
        转向\n
        参数：
            direction：方向（0：向左；1：向右）\n
            angle：角度
        返回：
            车当前的角度
        """
        old_angle = self.world_angle
        if direction == 0:
            old_angle -= angle
        if direction == 1:
            old_angle += angle

        self.set_angle(old_angle % 360)

        return self.world_angle

    def move(self, direction, distance):
        """
        移动\n
        参数：
            direction：方向（0：向前；1：向后）\n
            distance：距离
        返回：
            车当前的坐标x，车当前的坐标y
        """
        world_x, world_y = self.world_x, self.world_y
        if self.world_angle == 0:
            if direction == 0:
                world_y += distance
            if direction == 1:
                world_y -= distance
            pass
        elif self.world_angle < 90:
            x = self.sin(self.world_angle)
            y = self.cos(self.world_angle)
            if direction == 0:
                world_x += x
                world_y += y
            if direction == 1:
                world_x -= x
                world_y -= y
            pass
        elif self.world_angle == 90:
            if direction == 0:
                world_x += distance
            if direction == 1:
                world_x -= distance
            pass
        elif self.world_angle < 180:
            x = self.sin(180 - self.world_angle)
            y = self.cos(180 - self.world_angle)
            if direction == 0:
                world_x += x
                world_y -= y
            if direction == 1:
                world_x -= x
                world_y += y
            pass
        elif self.world_angle == 180:
            if direction == 0:
                world_y -= distance
            if direction == 1:
                world_y += distance
            pass
        elif self.world_angle < 270:
            x = self.sin(self.world_angle - 180)
            y = self.cos(self.world_angle - 180)
            if direction == 0:
                world_x -= x
                world_y -= y
            if direction == 1:
                world_x += x
                world_y += y
            pass
        elif self.world_angle == 270:
            if direction == 0:
                world_x -= distance
            if direction == 1:
                world_x += distance
            pass
        elif self.world_angle < 360:
            x = self.sin(360 - self.world_angle)
            y = self.cos(360 - self.world_angle)
            if direction == 0:
                world_x -= x
                world_y += y
            if direction == 1:
                world_x += x
                world_y -= y
            pass

        self.set_world_xy(world_x, world_y)

        return self.world_x, self.world_y

    def show_info(self, show):
        self.guid.show_car_position = show
        return show

    def target_update(self, target_x, target_y):
        self.target.update_position(target_x, target_y)
        self.has_target = True
        return self.target.world_x, self.target.world_y

    def target_remove(self):
        self.has_target = False

    def target_get(self):
        return self.has_target, self.target.world_x, self.target.world_y

    def sin(self, angle) -> float:
        """
        计算角度的正弦
        参数：
            angle：角度
        返回：
            正弦值
        """
        return math.sin(math.radians(angle))

    def cos(self, angle) -> float:
        """
        计算角度的余弦
        参数：
            angle：角度
        返回：
            余弦值
        """
        return math.cos(math.radians(angle))

    def winx_to_worldx(self, win_x: int) -> int:
        """
        窗口坐标x转换为世界坐标x
        参数：
            win_x：窗口坐标x
        返回：
            世界坐标x
        """
        win_width = self.screen.get_rect().width
        world_left = int(self.world_x - win_width / 2)
        world_x = world_left + win_x
        return world_x

    def winy_to_worldy(self, win_y) -> int:
        """
        窗口坐标y转换为世界坐标y
        参数：
            win_y：窗口坐标y
        返回：
            世界坐标y
        """
        win_height = self.screen.get_rect().height
        world_top = int(self.world_y + win_height / 2)
        world_y = world_top + win_y
        return world_y

    def win_rect_to_world_rect(self, win_rect) -> pygame.Rect:
        """
        窗口坐标矩形转换为世界坐标矩形
        参数：
            win_rect：窗口坐标矩形
        返回：
            世界坐标矩形
        """
        world_x = self.winx_to_worldx(win_rect.x)
        world_y = self.winy_to_worldy(win_rect.y)
        world_rect = pygame.Rect(world_x, world_y, win_rect.width, win_rect.height)
        return world_rect

    def worldx_to_winx(self, worldx) -> int:
        """
        世界坐标x转换为窗口坐标x
        参数：
            world_x：世界坐标x
        返回：
            窗口坐标x
        """
        win_width = self.screen.get_rect().width
        world_left = int(self.world_x - win_width / 2)
        win_x = worldx - world_left
        return win_x

    def worldy_to_winy(self, worldy) -> int:
        """
        世界坐标y转换为窗口坐标y
        参数：
            world_y：世界坐标y
        返回：
            窗口坐标y
        """
        win_height = self.screen.get_rect().height
        world_top = int(self.world_y + win_height / 2)
        win_y = world_top - worldy
        return win_y

    def world_rect_to_win_rect(self, world_rect) -> pygame.Rect:
        """
        世界坐标矩形转换为窗口坐标矩形
        参数：
            world_rect：世界坐标矩形
        返回：
            窗口坐标矩形
        """
        win_x = self.worldx_to_winx(world_rect.x)
        win_y = self.worldy_to_winy(world_rect.y)
        win_rect = pygame.Rect(win_x, win_y, world_rect.width, world_rect.height)
        return win_rect


class CarSrv(Node):
    def __init__(self, node_name, car: Car):
        super().__init__(node_name)
        self.car = car
        self._turn_action_server = ActionServer(
            self, Turn, "/car/turn", self.turn_callback
        )
        self._move_action_server = ActionServer(
            self, Move, "/car/move", self.move_callback
        )
        self._info_show_srv = self.create_service(
            InfoShow, "/car/info/show", self.info_show_callback
        )
        self._car_info_get_srv = self.create_service(
            CarInfoGet, "/car/info/get", self.car_info_get_callback
        )
        self._target_update_srv = self.create_service(
            TargetUpdate, "/car/target/update", self.target_update_callback
        )
        self._target_remove_srv = self.create_service(
            TargetRemove, "/car/target/remove", self.target_remove_callback
        )
        self._target_get_srv = self.create_service(
            TargetGet, "/car/target/get", self.target_get_callback
        )
        self._car_info_publisher = self.create_publisher(CarInfo, "/car/info", 10)

    def send_car_info(self, world_x, world_y, world_angle):
        msg = CarInfo()
        msg.world_x, msg.world_y, msg.world_angle = world_x, world_y, world_angle
        self._car_info_publisher.publish(msg)
        self.get_logger().info(
            f"/car/info publish ({msg.world_angle:.2f}, {msg.world_x:.2f}, {msg.world_y:.2f})"
        )
        pass

    def target_remove_callback(self, request, response):
        self.car.target_remove()
        self.get_logger().info(f"car/target/remove")
        return response

    def target_update_callback(self, request, response):
        response.target_x, response.target_y = self.car.target_update(
            request.x, request.y
        )
        self.get_logger().info(f"car/target/update request ({request.x}, {request.y})")
        return response

    def target_get_callback(self, request, response):
        response.has_target, response.world_x, response.world_y = self.car.target_get()
        self.get_logger().info(
            f"car/target/get response ({response.has_target}, {response.world_x}, {response.world_y})"
        )
        return response

    def info_show_callback(self, request, response):
        response.final_show = self.car.show_info(request.show)
        self.get_logger().info(f"/car/info/show request ({request.show})")
        return response

    def car_info_get_callback(self, request, response):
        response.world_x, response.world_y, response.world_angle = (
            self.car.world_x,
            self.car.world_y,
            self.car.world_angle,
        )
        self.get_logger().info(
            f"car/info/get response ({response.world_angle:.2f}, {response.world_x:.2f}, {response.world_y:.2f})"
        )
        return response

    def turn_callback(self, goal_handle):
        self.get_logger().info("Executing turn goal...")

        feedback_msg = Turn.Feedback()

        angle = goal_handle.request.angle
        iangle = math.floor(angle)  # 整数部分
        for i in range(1, iangle + 1):
            feedback_msg.world_angle = self.car.turn(goal_handle.request.direction, 1)
            feedback_msg.angle = float(i)
            self.get_logger().info(
                f"/car/turn feedback: ({feedback_msg.angle:.2f},{feedback_msg.world_angle:.2f})"
            )
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.01)

        dangle = angle % 1  # 小数部分
        if dangle != 0:
            feedback_msg.world_angle = self.car.turn(
                goal_handle.request.direction, dangle
            )
            feedback_msg.angle = dangle
            self.get_logger().info(
                f"/car/turn feedback: ({feedback_msg.angle:.2f},{feedback_msg.world_angle:.2f})"
            )
            goal_handle.publish_feedback(feedback_msg)

        goal_handle.succeed()
        result = Turn.Result()
        result.angle = feedback_msg.angle
        result.world_angle = feedback_msg.world_angle
        return result

    def move_callback(self, goal_handle):
        self.get_logger().info("Executing move goal...")

        feedback_msg = Move.Feedback()

        distance = goal_handle.request.distance
        idistance = math.floor(distance)
        for i in range(1, idistance + 1):
            feedback_msg.world_x, feedback_msg.world_y = self.car.move(
                goal_handle.request.direction, 1
            )
            feedback_msg.distance = float(i)
            self.get_logger().info(
                f"Feedback: ({feedback_msg.distance:.2f},{feedback_msg.world_x:.2f},{feedback_msg.world_y:.2f})"
            )
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.01)
        ddistance = distance % 1
        if ddistance != 0:
            feedback_msg.world_x, feedback_msg.world_y = self.car.move(
                goal_handle.request.direction, ddistance
            )
            feedback_msg.distance = ddistance
            self.get_logger().info(
                f"Feedback: ({feedback_msg.distance:.2f},{feedback_msg.world_x:.2f},{feedback_msg.world_y:.2f})"
            )
            goal_handle.publish_feedback(feedback_msg)

        goal_handle.succeed()
        result = Move.Result()
        result.distance = feedback_msg.distance
        result.world_x = feedback_msg.world_x
        result.world_y = feedback_msg.world_y
        return result


def main(args=None):
    car = Car()
    car.run_game()


if __name__ == "__main__":
    # sys.argv[1:]为要处理的参数列表，sys.argv[0]为脚本名，所以用sys.argv[1:]过滤掉脚本名。
    main(sys.argv[1:])
